#include <cstring>
#include <stdarg.h>
#include <thread>
#include <iostream>
#include <sstream>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <fcntl.h>
#include "LpmsIG1I.h"
#include "SensorDataI.h"
#include "LpmsIG1Registers.h"

//add
#include "LpmsIG1.h"
#define staticFileNameLength 50
using namespace std;
string TAG("MAIN");
IG1I* sensor1;
// Start print data thread
std::thread *printThread;
static bool printThreadIsRunning = false;

void logd(std::string tag, const char* str, ...)
{
    va_list a_list;
    va_start(a_list, str);
    if (!tag.empty())
        printf("[ %4s] [ %-8s]: ", "INFO", tag.c_str());
    vprintf(str, a_list);
    va_end(a_list);
} 

//2021-12-18 add 



void lenToHex(char *des,long len){
	des[0] = len&0x00ff;
       des[1] = (len>>8)&0x00ff;
       des[2] = (len>>16)&0x00ff;
       des[3] = (len>>24)&0x00ff;
}


void BCC(char *data,char *res){
	int len = strlen(data);
	char checknum = '\0';
	int i=1;
	for(i=1;i<len;i++){
		checknum = checknum ^ data[i];
	}
	
	sprintf(res,"%02x",checknum&0x00ff);
	
}




void getFileName(char *filename){
	
	FILE *fp;
	fp = fopen("filenameTemp.txt","rb");
  	while(fp==NULL){
  		usleep(30);
  		fp = fopen("filenameTemp.txt","rb");
  	} 
  	
	int re = fscanf(fp,"%[^\n]", filename);
	if(!re) perror("open filenameTemp.txt error");
	
	printf("filename %s\n",filename);
	fclose(fp);
	
}




void printTask()
{
    if (sensor1->getStatus() != STATUS_CONNECTED)
    {
        printThreadIsRunning = false;
        logd(TAG, "Sensor is not connected. Sensor Status: %d\n", sensor1->getStatus());
        logd(TAG, "printTask terminated\n");
        return;
    }
    
    sensor1->commandGotoStreamingMode();
    
    printThreadIsRunning = true;
    while (sensor1->getStatus() == STATUS_CONNECTED && printThreadIsRunning)
    {
        IG1ImuDataI sd;
        if (sensor1->hasImuData())
        {
            sensor1->getImuData(sd);
            float freq = sensor1->getDataFrequency();
            /*
            logd(TAG, "t(s): %.3f acc: %+2.2f %+2.2f %+2.2f gyr: %+3.2f %+3.2f %+3.2f euler: %+3.2f %+3.2f %+3.2f Hz:%3.3f \r\n", 
                sd.timestamp*0.002f, 
                sd.accCalibrated.data[0], sd.accCalibrated.data[1], sd.accCalibrated.data[2],
                sd.gyroIAlignmentCalibrated.data[0], sd.gyroIAlignmentCalibrated.data[1], sd.gyroIAlignmentCalibrated.data[2],
                sd.euler.data[0], sd.euler.data[1], sd.euler.data[2], 
                freq);
                */
                 logd(TAG, "t(s): %d acc: %+2.2f %+2.2f %+2.2f gyr: %+3.2f %+3.2f %+3.2f euler: %+3.2f %+3.2f %+3.2f magraw: %f %f %f quat: %f %f %f %f Hz:%3.3f \r\n", 
                sd.timestamp, 
                sd.accRaw.data[0], sd.accRaw.data[1], sd.accRaw.data[2],
                sd.gyroIRaw.data[0], sd.gyroIRaw.data[1], sd.gyroIRaw.data[2],
                sd.euler.data[0], sd.euler.data[1], sd.euler.data[2],sd.magRaw.data[0],sd.magRaw.data[1],sd.magRaw.data[2], sd.quaternion.data[0],sd.quaternion.data[1],sd.quaternion.data[2],sd.quaternion.data[3],
                freq);
                
                //get file
                
		 char filename[30] = { 0 };
		 
		 getFileName(filename);
		 char path[100] = {0};
		printf("lpms getfilename %s\n",filename);
		sprintf(path,"/media/ubuntu/bae/%s/lpms",filename);
		 
		 char *databuff = new char[100];
		
                struct timeval tv;
                gettimeofday(&tv,NULL);
                long ms = tv.tv_usec/1000;
                time_t curtime = time(nullptr);
		//char total_time[40];//
                //getTotalTime(total_time);//
		//sprintf(databuff,"$%d,%s,%d,%d,%d,%d,%d,%d,%d",sd.timestamp,total_time,(int)(sd.quaternion.data[0]*10000),(int)(sd.quaternion.data[1]*10000),(int)(sd.quaternion.data[2]*10000),(int)(sd.quaternion.data[3]*10000),(int)(sd.euler.data[0]*100),(int)(sd.euler.data[1]*100),(int)(sd.euler.data[2]*100));
		
                sprintf(databuff,"$%d,%ld,%d,%d,%d,%d,%d,%d,%d,%ld",sd.timestamp,curtime,(int)(sd.quaternion.data[0]*10000),(int)(sd.quaternion.data[1]*10000),(int)(sd.quaternion.data[2]*10000),(int)(sd.quaternion.data[3]*10000),(int)(sd.euler.data[0]*100),(int)(sd.euler.data[1]*100),(int)(sd.euler.data[2]*100),ms);
                
                char *checkSum = new char[4];
                BCC(databuff,checkSum);
                printf("checkSum %s\n",checkSum);
                char *temp = new char[4];
                sprintf(temp,"*%s",checkSum);
                strcat(databuff,temp);
                printf("databuff %s\n",databuff);
                int len = strlen(databuff);
                
                char hexlen[5];
                lenToHex(hexlen,len);
                
                package pack;
                pack.header = 0x7b;
                pack.kind = 0x02;
                pack.length_07 = hexlen[0];
                pack.length_815 = hexlen[1];
                pack.length_1623 = hexlen[2];
                pack.length_2431 = hexlen[3];
                pack.tail1 = 0x0d;
                pack.tail2 = 0x0a;
                
             
		
		
                FILE *fp;
		 fp = fopen64(path ,"ab+");
                fwrite(&pack.header,sizeof(char),1,fp);
                fwrite(&pack.kind,sizeof(char),1,fp);
		fwrite(&pack.length_07,sizeof(char),1,fp);
		fwrite(&pack.length_815,sizeof(char),1,fp);
		fwrite(&pack.length_1623,sizeof(char),1,fp);
		fwrite(&pack.length_2431,sizeof(char),1,fp);
		fwrite(databuff,strlen(databuff),1,fp);
		fwrite(&pack.tail1,sizeof(char),1,fp);
		fwrite(&pack.tail2,sizeof(char),1,fp);
		fclose(fp);
		
		delete databuff;
		delete checkSum;
		delete temp;
		
		
		
                
                
                
        }
        this_thread::sleep_for(chrono::milliseconds(2));
    }

    printThreadIsRunning = false;
    logd(TAG, "printTask terminated\n");
}

void printMenu()
{
    cout <<  endl;
    cout << "===================" << endl;
    cout << "Main Menu" << endl;
    cout << "===================" << endl;
    cout << "[c] Connect sensor" << endl;
    cout << "[d] Disconnect sensor" << endl;
    cout << "[0] Goto command mode" << endl;
    cout << "[1] Goto streaming mode" << endl;
    cout << "[h] Reset sensor heading" << endl;
    cout << "[i] Print sensor info" << endl;
    cout << "[s] Print sensor settings" << endl;
    cout << "[p] Print sensor data" << endl;
    cout << "[a] Toggle enable/disable auto reconnect" << endl;
    cout << "[q] quit" << endl;
    cout << endl;
}

int main(int argc, char** argv)
{
    string comportNo = "LPMSIG1-SENSOR-ID";
    int baudrate = 921600;
    
    if (argc == 2)
    {
        comportNo = string(argv[1]);
    }
    else if (argc == 3)
    {
        comportNo = string(argv[1]);
        baudrate = atoi(argv[2]);
    }
	
	logd(TAG, "Connecting to %s @%d\n", comportNo.c_str(), baudrate);
    // Create LpmsIG1 object with corresponding comport and baudrate
    sensor1 = IG1Factory();

    sensor1->setVerbose(VERBOSE_INFO);
    sensor1->setAutoReconnectStatus(false);
    int flag = -1;
    while(1)
    {
    	
    	flag = sensor1->connect(comportNo, baudrate);
	if(flag==2) {
		logd(TAG, "Sensor connected\n");
		logd(TAG, "Print sensor data\n"); 
		  // Print sensor data
    		
        	printThread = new std::thread(printTask);
        	if (printThread && printThread->joinable()) {
                	printThread->join();
                	printThread = NULL;
            	}
	}
	else{
		
		logd(TAG, "Sensor connection error status: %d\n", sensor1->getStatus());
        	//sensor1->release();
        	printf("%d\n",sensor1->getStatus());
        	logd(TAG, "Waiting for sensor to connect\r\n");
        	this_thread::sleep_for(chrono::milliseconds(100));
        	
	}
		
	        
	
        
    } 
	/*
    if (sensor1->getStatus() != STATUS_CONNECTED)
    {
        logd(TAG, "Sensor connection error status: %d\n", sensor1->getStatus());
        sensor1->release();
        logd(TAG, "bye\n");
        return 1;
    }
*/

    
    //2021-12-18 add
    
   
        
    /*
    printMenu();

    bool quit = false;
    while (!quit)
    {
        char cmd = '\0';
        string input;
        getline(cin, input);
        
        if (!input.empty())
            cmd = input.at(0);
            
        switch (cmd)
        {
        case 'c':
            logd(TAG, "Connect sensor\n");
            if (!sensor1->connect(comportNo, baudrate))
                logd(TAG, "Error connecting to sensor\n");
            break;

        case 'd':
            logd(TAG, "Disconnect sensor\n");
            sensor1->disconnect();
            break;

        case '0':
            logd(TAG, "Goto command mode\n");
            sensor1->commandGotoCommandMode();
            //logd(TAG,"commandSetUartDataFormat\n");
            sensor1->commandSetUartDataFormat(1);
            
            
            break;

        case '1':
            logd(TAG, "Goto streaming mode\n");
            sensor1->commandGotoStreamingMode();
            break;

        case 'h':
            logd(TAG, "Reset heading\n");
            // reset sensor heading
            sensor1->commandSetOffsetMode(LPMS_OFFSET_MODE_HEADING);
            break;

        case 'i': {
            logd(TAG, "Print sensor info\n");
            // Print sensor info
            IG1InfoI info;
            sensor1->getInfo(info);
            cout << info.toString() << endl;
            break;
        }

        case 's': {
            logd(TAG, "Print sensor settings\n");
            // Print sensor settings
            IG1SettingsI settings;
            sensor1->getSettings(settings);
            
            cout << settings.toString() << endl;
            break;
        }

        case 'p': {     
            logd(TAG, "Print sensor data\n");       
            // Print sensor data
            if (!printThreadIsRunning)
                printThread = new std::thread(printTask);
            break;
        }
	
        case 'a': {
            logd(TAG, "Toggle auto reconnect\n");
            bool b = sensor1->getAutoReconnectStatus();
            sensor1->setAutoReconnectStatus(!b);
            logd(TAG, "Auto reconnect %s\n", !b? "enabled":"disabled");
            break;
        }
      
        case 'q':
            logd(TAG, "quit\n");
            // disconnect sensor
            sensor1->disconnect();
            quit = true;
            break;
        
        	

        default:
            printThreadIsRunning = false;
	        if (printThread && printThread->joinable()) {
                printThread->join();
                printThread = NULL;
            }
            printMenu();
          break;
        }
        this_thread::sleep_for(chrono::milliseconds(100));
    }
	*/
	
    //if (printThread && printThread->joinable())
      //  printThread->join();

    // release sensor resources
    //sensor1->release();
    //logd(TAG, "Bye\n");

    return 0;
}
